#include <ros/ros.h>
#include "walk.h"
#include "webots_api/SceneSelection.h"
#include <std_msgs/String.h>
#include <sensor_msgs/JointState.h>

void data_to_leg(sensor_msgs::JointState &joint_state, float *joints)
{
    joint_state.position[0] = joints[0];
    joint_state.position[1] = joints[1];
    joint_state.position[2] = joints[2];
    joint_state.position[3] = joints[3];
    joint_state.position[4] = joints[4];
    joint_state.position[5] = joints[5];
    joint_state.position[6] = joints[6];
    joint_state.position[7] = joints[7];
    joint_state.position[8] = joints[8];
    joint_state.position[9] = joints[9];
    joint_state.position[10] = joints[10];
    joint_state.position[11] = joints[11];
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "Upstairs_node");
    ros::NodeHandle nh;

    ros::ServiceClient client = nh.serviceClient<webots_api::SceneSelection>("/walker/sence");
        
    webots_api::SceneSelection scene_selection;
    scene_selection.request.scene_name = "Upstairs";
    scene_selection.request.nav = false;
    scene_selection.request.vision = false;
    
    std::cout << "Wait for SceneSelect Service" << std::endl;
    while(!client.call(scene_selection))
    {
        if(!ros::ok())
        {
            return 0;
        }
    }
    ROS_INFO("Scene select success!");

    ros::Publisher leg_pub = nh.advertise<sensor_msgs::JointState>("/Leg/DesiredJoint", 10);
    sensor_msgs::JointState leg_joints;
    leg_joints.position.resize(12);

    Walk* leg_walk = new Walk();
    float joints[12] = {0};

    ros::Rate loop_rate(1000);
    while(ros::ok())
    {
        if(leg_walk->get_status() == "standing")
        {
            leg_walk->Init(joints, 91);
            data_to_leg(leg_joints, joints);
            leg_pub.publish(leg_joints);
        }
        else
        {
            if(leg_walk->walk(joints) == true)
            {
                data_to_leg(leg_joints, joints);
                leg_pub.publish(leg_joints);
            }
        }
        
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}